.global v7_dma_inv_range
.global v7_dma_clean_range
.global setjmp
.global longjmp
.global _platform_setup1

.extern hal_mmu_init
.extern board_config

.fpu vfpv4
.arch armv7a
      .macro  dcache_line_size, reg, tmp
    mrc     p15, 0, \tmp, c0, c0, 1
    lsr     \tmp, \tmp, #16
    and     \tmp, \tmp, #0xf
    mov     \reg, #4
    mov     \reg, \reg, lsl \tmp
    .endm

     .macro FLUSH_DCACHE_ALL
     mrc     p15, 0, r8, c0, c1, 5
     tst     r8, #0xf << 16
     mov     r8, #0
     beq     Hierarchical
     mcr     p15, 0, r8, c7, c14, 0
     b       Iflush
     Hierarchical:
     mcr     p15, 0, r8, c7, c10, 5
     stmfd   sp!, {r0-r11}
     mrc     p15, 1, r0, c0, c0, 1
     ands    r3, r0, #0x7000000
     mov     r3, r3, lsr #24
     beq     Finished
     mov     r8, #0

     Loop1:
     and     r1, r0, #7
     cmp     r1, #2
     blt     Skip

     mov     r11, r8, lsl #1
     mcr     p15, 2, r11, c0, c0, 0
     mcr     p15, 0, r11, c7, c5, 4
     mrc     p15, 1, r1, c0, c0, 0

     and     r2, r1, #7
     add     r2, r2, #4
     ldr     r4, =0x3ff
     ands    r4, r4, r1, lsr #3
     clz     r5, r4
     ldr     r7, =0x7fff
     ands    r7, r7, r1, lsr #13
     Loop2:
     mov     r9, r4
     Loop3:
     orr     r6, r11, r9, lsl r5
     orr     r6, r6, r7, lsl r2
     mcr     p15, 0, r6, c7, c14, 2
     subs    r9, r9, #1
     bge     Loop3
     subs    r7, r7, #1
     bge     Loop2
     Skip:
     mov     r0, r0, lsr #3
     add     r8, r8, #1
     cmp     r3, r8
     bgt      Loop1
     Finished:
     dsb
     ldmia   sp!, {r0-r11}
     mov     r8, #0
     mcr     p15, 2, r8, c0, c0, 0
     Iflush:
     mcr     p15, 0, r8, c7, c10, 4
     mcr     p15, 0, r8, c7, c5, 0
     mcr     p15, 0, r8, c7, c10, 4
     mcr     p15, 0, r8, c7, c5, 4
     .endm



_platform_setup1:
    push {lr}

    bl  board_config
    mrc  p15,0,r0,c1,c0,0
    bic  r0,r0,#0x1000 //disable ICache [SCTRL:bit 12 set as 0]
    bic  r0,r0,#0x000f //disable DCache, write buffer,

    mcr  p15,0,r0,c1,c0,0
    nop
    nop
    mcr  p15,0,r0,c1,c0,0
    nop
    nop
    mov  r0,#0

    FLUSH_DCACHE_ALL

    bl  hal_mmu_init

    mrc     p15, 0, r0, c1, c0, 1 //ACTLR, Auxlliary Control Register, IMPLEMENTATION DEFINED
    orr     r0, #(1 << 6)  //SMP, Signals if the Cortex-A9 processor is taking part in coherency or not.
    mcr     p15, 0, r0, c1, c0, 1 //ACTLR, Auxlliary Control Register, IMPLEMENTATION DEFINED


    ldr     r2,=10f

    mov     r0, #0
    mcr     p15, 0, r0, c7, c10, 4 //CP15DSB, Data Synchronization Barrier operation
    tst     r11, #0xf
    mcrne   p15, 0, r0, c8, c7, 0 //TBLIALL, invalidate unified TLB

    mrc     p15, 0, r0, c1, c0, 0 //SCTLB, System Control Register
    orr     r0, r0, #1<<12 // enable ICache
    orr     r0, r0, #1<<2 //Dcache enable
    orr     r0, r0, #1<<8 // 'S' bit
    //orr     r0, r0, #1<<1 // not check align
    orr     r0, r0, #1

    mcr     p15, 0, r0, c7, c5, 4 //CP15ISB, Instruction Synchronization Barrier operation
    mcr     p15, 0, r0, c1, c0, 0 //SCTLB, System Control Register
    mrc     p15, 0, r0, c1, c0, 0 //SCTLB, System Control Register

    mov     r0, #0
    mcr     p15, 0, r0, c7, c5, 4 //CP15ISB, Instruction Synchronization Barrier operation

    mov        pc,r2
    nop
    nop
    nop
10:
    @b   warm_reset
    pop  {pc}


v7_dma_inv_range:
    push    {r2, r3, ip}
    dcache_line_size ip, r3
    sub     r3, ip, #1
    tst     r0, r3
    bic     r0, r0, r3
    mcrne   p15, 0, r0, c7, c14, 1
    tst     r1, r3
    bic     r1, r1, r3
    mcrne   p15, 0, r1, c7, c14, 1

    sub     r3, ip, #1
    and     r2, r0, r3
    sub     r1, r1, r0
    add     r1, r1, r2
.Larmv7_inv_next:
    mcr     p15, 0, r0, c7, c6, 1
    add     r0, r0, ip
    subs    r1, r1, ip
    bhi     .Larmv7_inv_next
    dsb
    pop    {r2, r3, ip}
    mov    pc, lr
.type v7_dma_inv_range,%function;
.size v7_dma_inv_range, .-v7_dma_inv_range



v7_dma_clean_range:
    push    {r2, r3, ip}
    dcache_line_size ip, r3
    sub     r3, ip, #1
    and     r2, r0, r3
    sub     r1, r1, r0
    add     r1, r1, r2
    bic     r0, r0, r3
.Larmv7_wb_next:
    mcr     p15, 0, r0, c7, c10, 1
    add     r0, r0, ip
    subs    r1, r1, ip
    bhi     .Larmv7_wb_next
    dsb
    pop    {r2, r3, ip}
    mov    pc, lr
.type v7_dma_clean_range,%function;
.size v7_dma_clean_range, .-v7_dma_clean_range



longjmp:

        ldmfd   r0,{r4-r14}
        cmp     r1,#0
        moveq   r1,#1
        mov     r0,r1
        mov     pc,lr

.type longjmp,%function;
.size longjmp, .-longjmp


setjmp:
        stmea   r0,{r4-r14}
        mov     r0,#0

        mov     pc,lr

.type setjmp,%function;
.size setjmp, .-setjmp

